#include <rclcpp/rclcpp.hpp>
#include <tf2/utils.h>
#include <tf2_ros/transform_broadcaster.h>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <nav_msgs/msg/odometry.hpp>


class OdomTopic2TF : public rclcpp::Node
{
    public:
        //OdomTopic2TF继承Node节点
        OdomTopic2TF(std::string name) : Node(name)
        {
            //创建一个ROS 2订阅器
            odom_subscribe_ = this->create_subscription<nav_msgs::msg::Odometry>(
                "odom",//订阅/odom话题的Odometry消息
                rclcpp::SensorDataQoS(),//使用ROS 2预定义的传感器数据QoS配置文件
                std::bind(&OdomTopic2TF::odom_callback_,this,std::placeholders::_1) //回调函数绑定
            );

            //用于广播的坐标变化
            tf_broadcaster_ = std::make_unique<tf2_ros::TransformBroadcaster>(this);
        }
    private:
        //声明一个ROS 2订阅器，用于接收nav_msgs::msg::Odometry类型的里程计消息。
        rclcpp::Subscription<nav_msgs::msg::Odometry>::SharedPtr odom_subscribe_;
        //声明一个TF2坐标变换广播器的独占指针，用于发布坐标系之间的变换关系
        std::unique_ptr<tf2_ros::TransformBroadcaster> tf_broadcaster_;

        // 回调函数，处理接收到的odom消息，并发布tf
        void odom_callback_(const nav_msgs::msg::Odometry::SharedPtr msg)
        {
            geometry_msgs::msg::TransformStamped transform;
            transform.header = msg->header; // 使用消息的时间戳和框架ID
            transform.child_frame_id = msg->child_frame_id;
            transform.transform.translation.x = msg->pose.pose.position.x;
            transform.transform.translation.y = msg->pose.pose.position.y;
            transform.transform.translation.z = msg->pose.pose.position.z;
            transform.transform.rotation.x = msg->pose.pose.orientation.x;
            transform.transform.rotation.y = msg->pose.pose.orientation.y;
            transform.transform.rotation.z = msg->pose.pose.orientation.z;
            transform.transform.rotation.w = msg->pose.pose.orientation.w;
            // 广播坐标变换信息
            tf_broadcaster_->sendTransform(transform);
        };

};



int main(int argc, char **argv) {
  rclcpp::init(argc, argv);
  auto node = std::make_shared<OdomTopic2TF>("odom2tf");
  rclcpp::spin(node);
  rclcpp::shutdown();
  return 0;
}